#include <stdio.h>
#include <stdint.h>
#include "FLOAT.h"
#ifdef LINUX_RT
#include <sys/mman.h>
#endif

extern char _vfprintf_internal;
extern char _fpmaxtostr;
extern char _ppfs_setargs;
extern int __stdio_fwrite(char *buf, int len, FILE *stream);

__attribute__((used)) static int format_FLOAT(FILE *stream, FLOAT f) {
	/* Format a Q16.16 FLOAT as decimal with 6 digits after the point,
	 * truncating (not rounding), and write to `stream`. */
	char buf[80];
	int negative = 0;
	int32_t v = f;
	if (v < 0) { negative = 1; v = -v; }
	int32_t int_part = (int32_t)((uint32_t)v >> 16);
	uint32_t frac_raw = (uint32_t)v & 0xFFFFu;
	/* scale fractional part to 10^6 with truncation */
	uint32_t frac = (uint32_t)(((uint64_t)frac_raw * 1000000u) >> 16);
	int len;
	if (negative) {
		len = sprintf(buf, "-%d.%06u", int_part, frac);
	} else {
		len = sprintf(buf, "%d.%06u", int_part, frac);
	}
	return __stdio_fwrite(buf, len, stream);
}

static void modify_vfprintf() {
    int base = (int)(&_vfprintf_internal);

#ifdef LINUX_RT
    /* Open write permission on relevant code pages. */
    void *page = (void *)((base + 0x306 - 100) & ~0xFFF);
    mprotect(page, 4096 * 2, PROT_READ | PROT_WRITE | PROT_EXEC);
#endif

	/* NOP out floating-point loads at offsets 0x2e4 and 0x2e8. */
	char *p8 = (char *)(base + 0x2e4);
	*p8++ = (char)0x90; /* nop */
	*p8   = (char)0x90; /* nop */

	p8 = (char *)(base + 0x2e8);
	*p8++ = (char)0x90; /* nop */
	*p8   = (char)0x90; /* nop */

	/* Adjust stack allocation from 12 to 8 bytes at offset 0x2f9. */
	p8 = (char *)(base + 0x2f9);
	*p8++ = (char)0x83; /* sub */
	*p8++ = (char)0xEC; /* esp, */
	*p8   = (char)0x08; /* 8 */

	/* Replace fstpt [esp] (3 bytes) with push dword [edx] + nop at 0x2fc. */
	p8 = (char *)(base + 0x2fc);
	*p8++ = (char)0xFF; /* push r/m32 */
	*p8++ = (char)0x32; /* ModR/M: [edx] */
	*p8   = (char)0x90; /* nop */

	/* Retarget call from _fpmaxtostr to format_FLOAT by fixing rel32 at 0x307. */
	int *rel32 = (int *)(base + 0x307);
	*rel32 += (int)format_FLOAT - (int)(&_fpmaxtostr);
}

static void modify_ppfs_setargs() {
    int base = (int)(&_ppfs_setargs);

#ifdef LINUX_RT
    /* Open write permission on the target code page. */
    void *page = (void *)((base + 0x71 - 100) & ~0xFFF);
    mprotect(page, 4096, PROT_READ | PROT_WRITE | PROT_EXEC);
#endif

	/* At 0x71, replace `lea ebx, [edx+0x8]` (8D 5A 08) with `jmp +0x30; nop`. */
	char *hijack = (char *)(base + 0x71);
	*hijack++ = (char)0xEB; /* short jump */
	*hijack++ = (char)0x30; /* to 0xA3 */
	*hijack   = (char)0x90; /* nop */
}

void init_FLOAT_vfprintf() {
	modify_vfprintf();
	modify_ppfs_setargs();
}